// darts_adjust_node.hpp

#ifndef DARTS_ADJUST_NODE_HPP
#define DARTS_ADJUST_NODE_HPP

#include "cv_bridge/cv_bridge.h"
#include "opencv2/opencv.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/msg/image.hpp"
#include <algorithm>
#include <image_transport/image_transport.hpp>
#include <iostream>
#include <memory>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include "std_msgs/msg/float64.hpp"

namespace darts_adjust {

class DartsAdjustNode : public rclcpp::Node {
public:
  explicit DartsAdjustNode(
      const rclcpp::NodeOptions &options = rclcpp::NodeOptions());

private:
  // 相机矩阵
  cv::Mat camera_matrix_;
  size_t frame_count_;
  rclcpp::Time last_time_;

  // 图像处理函数
  cv::Mat processImage(const cv::Mat &input_image);

  // 订阅者
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr darts_image_sub_;
  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub_;

  // 发布者
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr error_pub_;
  // 添加调试图像发布者
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr debug_image_pub_;

  // TF2 缓存和监听器
  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;

  // 目标像素差距
  int target_error_;
  
  // 卡尔曼滤波器
  cv::KalmanFilter kf_;
  cv::Mat measurement_;
  bool kalman_initialized_;
  void initKalmanFilter();

  // 回调函数
  void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg);
};

} // namespace darts_adjust

#endif // DARTS_ADJUST_NODE_HPP